#include "CoordSys.h"
#include <cmath>

// BLHתXYZ 
XYZ CoordSys::BLH2XYZ(const BLH& blh) const {
    const double a = ellipsoid_.a;
    const double f = ellipsoid_.f;
    const double e2 = 2 * f - f * f;

    const double sinp = sin(blh.lat);
    const double cosp = cos(blh.lat);
    const double sinl = sin(blh.lon);
    const double cosl = cos(blh.lon);

    const double n = a / sqrt(1.0 - e2 * sinp * sinp);

    return {
        (n + blh.height) * cosp * cosl,
        (n + blh.height) * cosp * sinl,
        (n * (1.0 - e2) + blh.height) * sinp
    };
}

// XYZתBLH
BLH CoordSys::XYZ2BLH(const XYZ& xyz) const {
    const double a = ellipsoid_.a;
    const double f = ellipsoid_.f;
    const double e2 = 2 * f - f * f;
    const double eps = 1e-12;
    const int max_iter = 10;

    const double r = sqrt(xyz.x * xyz.x + xyz.y * xyz.y);
    double zk = 0.0;
    double sinp = 0.0, n = 0.0;

    BLH blh;
    blh.lon = atan2(xyz.y, xyz.x);

    for (int i = 0; i < max_iter; i++) {
        zk = xyz.z + e2 * n * sinp;
        sinp = zk / sqrt(r * r + zk * zk);
        const double cosp = r / sqrt(r * r + zk * zk);
        n = a / sqrt(1.0 - e2 * sinp * sinp);
        if (fabs(zk - (xyz.z + e2 * n * sinp)) < eps) break;
    }

    blh.lat = atan(zk / r);
    blh.height = r / cos(blh.lat) - n;

    return blh;
}

// XYZתENU
ENU CoordSys::XYZ2ENU(const XYZ& xyz, const BLH& ref_blh) const {
    const XYZ ref_xyz = BLH2XYZ(ref_blh);
    const XYZ diff = XYZSubtract(xyz, ref_xyz);

    const double sinp = sin(ref_blh.lat);
    const double cosp = cos(ref_blh.lat);
    const double sinl = sin(ref_blh.lon);
    const double cosl = cos(ref_blh.lon);

    return {
        -sinl * diff.x + cosl * diff.y,
        -sinp * cosl * diff.x - sinp * sinl * diff.y + cosp * diff.z,
        cosp * cosl * diff.x + cosp * sinl * diff.y + sinp * diff.z
    };
}